/*
 * Copyright (c) 2008, Keith Woodward
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 * 3. Neither the name of Keith Woodward nor the names
 *    of its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 */
package batailledesardennes.util.straightedge.path;

import batailledesardennes.util.straightedge.KPoint;
import batailledesardennes.util.straightedge.KPolygon;
import batailledesardennes.util.straightedge.util.BinaryHeap;
import batailledesardennes.util.straightedge.util.TileArray;
import batailledesardennes.util.straightedge.util.TileBag;
import batailledesardennes.util.straightedge.util.Tracker;
import java.util.*;

/**
 * Finds a path through PathBlockingObstacles.
 *
 * Some code concepts from: http://www.policyalmanac.org/games/aStarTutorial.htm
 * Thanks to Jono and Nate (Nathan Sweet) from JavaGaming.org for their clever help.
 *
 * @author Keith Woodward
 *
 * @version 20 December 2008
 */
public class PathFinder {
   public KNode startNode;
   public KNode endNode;
   public BinaryHeap<KNode> openList;
   // Tracker is used in conjunction with the KNodes to detect if the Nodes are in the open or closed state.
   Tracker tracker = new Tracker();
   // for debugging only:
   public boolean debug = false;
   public KPoint startPointDebug;
   public KPoint endPointDebug;
   public ArrayList<KNode> startNodeTempReachableNodesDebug = new ArrayList<KNode>();
   public ArrayList<KNode> endNodeTempReachableNodesDebug = new ArrayList<KNode>();

   public PathFinder() {
      openList = new BinaryHeap<KNode>();
      startNode = new KNode();
      endNode = new KNode();
   }

   public List<KPoint> calc(KPoint start, KPoint end, double maxConnectionDistance, NodeConnector nodeConnector, List<PathBlockingObstacle> obstacles) {
      List<KPoint> pointList = new ArrayList<KPoint>();
      return calc(start, end, maxConnectionDistance, nodeConnector, obstacles, pointList);
   }

   public List<KPoint> calc(KPoint start, KPoint end, double maxConnectionDistance, NodeConnector nodeConnector, List<PathBlockingObstacle> obstacles, List<KPoint> pointList) {
      pointList.clear();
      //double startToEndDist = start.distance(end);
      //KPoint midPoint = start.midPoint(end);
      //ArrayList obstacles = obstaclesTileArray.getAllWithin(midPoint, startToEndDist/2f);
      assert tempReachableNodesExist(obstacles) == false;

      startNode.clearForReuse();
      startNode.setPoint(start);
      // Set startNode gCost to zero
      startNode.calcGCost();
      KNode currentNode = startNode;
      endNode.clearForReuse();
      endNode.setPoint(end);

      // Check for straight line path between start and end.
      // Note that this assumes start and end are not both contained in the same polygon.
      boolean intersection = false;
      for (int i = 0; i < obstacles.size(); i++) {
         KPolygon innerPolygon = ((PathBlockingObstacle) obstacles.get(i)).getInnerPolygon();
         // Test if polygon intersects the line from start to end
         if (innerPolygon.intersectionPossible(start, end) && innerPolygon.intersectsLine(start, end)) {
            intersection = true;
            break;
         }
      }
      if (intersection == false) {
         // No intersections, so the straight-line path is fine!
         endNode.setParent(currentNode);
         fillPointList(pointList);
         clearTempReachableNodes();
         tracker.incrementCountID();
         return pointList;
      }
      {
         // Connect the startNode to its reachable nodes and vice versa
         List<KNode> reachableNodes = nodeConnector.makeReachableNodesFor(startNode, maxConnectionDistance, obstacles);
         startNode.getTempConnectedNodes().addAll(reachableNodes);
         for (int i = 0; i < reachableNodes.size(); i++) {
            KNode node = reachableNodes.get(i);
            node.getTempConnectedNodes().add(startNode);
         }

         // Connect the endNode to its reachable nodes and vice versa
         reachableNodes = nodeConnector.makeReachableNodesFor(endNode, maxConnectionDistance, obstacles);
         endNode.getTempConnectedNodes().addAll(reachableNodes);
         for (int i = 0; i < reachableNodes.size(); i++) {
            KNode node = reachableNodes.get(i);
            node.getTempConnectedNodes().add(endNode);
         }
      }

      // Here we start the A* algorithm!
      openList.makeEmpty();
      while (true) {
         // put the current node in the closedSet and take it out of the openList.
         currentNode.setPathFinderStatus(KNode.CLOSED, tracker);
         if (!openList.isEmpty()) {
            openList.deleteMin();
         }
         // add reachable nodes to the openList if they're not already there.
         List<KNode> reachableNodes = currentNode.getConnectedNodes();
         for (int i = 0; i < reachableNodes.size(); i++) {
            KNode reachableNode = reachableNodes.get(i);
            if (reachableNode.getPathFinderStatus(tracker) == KNode.UNPROCESSED) {
               reachableNode.setParent(currentNode);
               reachableNode.calcHCost(endNode);
               reachableNode.calcGCost();
               reachableNode.calcFCost();
               openList.add(reachableNode);
               reachableNode.setPathFinderStatus(KNode.OPEN, tracker);
            } else if (reachableNode.getPathFinderStatus(tracker) == KNode.OPEN) {
               assert reachableNode.getGCost() != KNode.G_COST_NOT_CALCULATED_FLAG;
               double currentGCost = reachableNode.getGCost();
               double newGCost = currentNode.getGCost() + currentNode.getPoint().distance(reachableNode.getPoint());
               if (newGCost < currentGCost) {
                  reachableNode.setParent(currentNode);
                  reachableNode.setGCost(newGCost);	//reachableNode.calcGCost();
                  reachableNode.calcFCost();
                  // Since the g-cost of the node has changed,
                  // must re-sort the list to reflect this.
                  int index = openList.indexOf(reachableNode);
                  openList.percolateUp(index);
               }
            }
         }
         for (int i = 0; i < currentNode.getTempConnectedNodes().size(); i++) {
            KNode reachableNode = currentNode.getTempConnectedNodes().get(i);
            if (reachableNode.getPathFinderStatus(tracker) == KNode.UNPROCESSED) {
               reachableNode.setParent(currentNode);
               reachableNode.calcHCost(endNode);
               reachableNode.calcGCost();
               reachableNode.calcFCost();
               openList.add(reachableNode);
               reachableNode.setPathFinderStatus(KNode.OPEN, tracker);
            } else if (reachableNode.getPathFinderStatus(tracker) == KNode.OPEN) {
               double currentGCost = reachableNode.getGCost();
               assert reachableNode.getGCost() != KNode.G_COST_NOT_CALCULATED_FLAG;
               double currentNodeToReachableNodeDist = currentNode.getPoint().distance(reachableNode.getPoint());
               double newGCost = currentNode.getGCost() + currentNodeToReachableNodeDist;
               if (newGCost < currentGCost) {
                  reachableNode.setParent(currentNode);
                  reachableNode.setGCost(newGCost);	//reachableNode.calcGCost();
                  reachableNode.calcFCost();
                  // Since the g-cost of the node has changed,
                  // must re-sort the list to reflect this.
                  int index = openList.indexOf(reachableNode);
                  openList.percolateUp(index);
               }
            }
         }
         if (openList.isEmpty()) {
            //System.out.println(this.getClass().getSimpleName()+": openList.size() == 0, returning");
            break;
         }

         currentNode = openList.peekMin();
         if (currentNode == endNode) {
            fillPointList(pointList);
            //System.out.println(this.getClass().getSimpleName()+": currentNode == endNode, returning");
            break;
         }
      }
      clearTempReachableNodes();
      tracker.incrementCountID();
      return pointList;
   }

   public ArrayList<KPoint> calc(KPoint start, KPoint end, double maxConnectionDistance, NodeConnector nodeConnector, TileBag obstaclesTileBag, ArrayList<KPoint> pointList) {
      return calc(start, end, maxConnectionDistance, nodeConnector, obstaclesTileBag.getTileArray(), pointList);
   }

   public ArrayList<KPoint> calc(KPoint start, KPoint end, double maxConnectionDistance, NodeConnector nodeConnector, TileArray obstaclesTileArray, ArrayList<KPoint> pointList) {
      pointList.clear();
      double startToEndDist = start.distance(end);
      KPoint midPoint = start.midPoint(end);
      ArrayList obstacles = obstaclesTileArray.getAllWithin(midPoint, startToEndDist / 2f);
      assert tempReachableNodesExist(obstacles) == false;

      startNode.clearForReuse();
      startNode.setPoint(start);
      // Set startNode gCost to zero
      startNode.calcGCost();
      KNode currentNode = startNode;
      endNode.clearForReuse();
      endNode.setPoint(end);

      // Check for straight line path between start and end.
      // Note that this assumes start and end are not both contained in the same polygon.
      boolean intersection = false;
      ObstacleLoop:
      for (int i = 0; i < obstacles.size(); i++) {
         KPolygon innerPolygon = ((PathBlockingObstacle) obstacles.get(i)).getInnerPolygon();
         // Test if polygon intersects the line from start to end
         if (innerPolygon.intersectionPossible(start, end) && innerPolygon.intersectsLine(start, end)) {
            intersection = true;
            break ObstacleLoop;
         }
      }
      if (intersection == false) {
         // No intersections, so the straight-line path is fine!
         endNode.setParent(currentNode);
         fillPointList(pointList);
         clearTempReachableNodes();
         tracker.incrementCountID();
         return pointList;
      }
      {
         // Connect the startNode to its reachable nodes and vice versa
         List<KNode> reachableNodes = nodeConnector.makeReachableNodesFor(startNode, maxConnectionDistance, obstaclesTileArray);
         startNode.getTempConnectedNodes().addAll(reachableNodes);
         for (int i = 0; i < reachableNodes.size(); i++) {
            KNode node = reachableNodes.get(i);
            node.getTempConnectedNodes().add(startNode);
         }

         // Connect the endNode to its reachable nodes and vice versa
         reachableNodes = nodeConnector.makeReachableNodesFor(endNode, maxConnectionDistance, obstaclesTileArray);
         endNode.getTempConnectedNodes().addAll(reachableNodes);
         for (int i = 0; i < reachableNodes.size(); i++) {
            KNode node = reachableNodes.get(i);
            node.getTempConnectedNodes().add(endNode);
         }
      }

      // Here we start the A* algorithm!
      openList.makeEmpty();
      while (true) {
         // put the current node in the closedSet and take it out of the openList.
         currentNode.setPathFinderStatus(KNode.CLOSED, tracker);
         if (openList.isEmpty() == false) {
            openList.deleteMin();
         }
         // add reachable nodes to the openList if they're not already there.
         ArrayList<KNode> reachableNodes = currentNode.getConnectedNodes();
         for (int i = 0; i < reachableNodes.size(); i++) {
            KNode reachableNode = reachableNodes.get(i);
            if (reachableNode.getPathFinderStatus(tracker) == KNode.UNPROCESSED) {
               reachableNode.setParent(currentNode);
               reachableNode.calcHCost(endNode);
               reachableNode.calcGCost();
               reachableNode.calcFCost();
               openList.add(reachableNode);
               reachableNode.setPathFinderStatus(KNode.OPEN, tracker);
            } else if (reachableNode.getPathFinderStatus(tracker) == KNode.OPEN) {
               assert reachableNode.getGCost() != KNode.G_COST_NOT_CALCULATED_FLAG;
               double currentGCost = reachableNode.getGCost();
               double newGCost = currentNode.getGCost() + currentNode.getPoint().distance(reachableNode.getPoint());
               if (newGCost < currentGCost) {
                  reachableNode.setParent(currentNode);
                  reachableNode.setGCost(newGCost);	//reachableNode.calcGCost();
                  reachableNode.calcFCost();
                  // Since the g-cost of the node has changed,
                  // must re-sort the list to reflect this.
                  int index = openList.indexOf(reachableNode);
                  openList.percolateUp(index);
               }
            }
         }
         for (int i = 0; i < currentNode.getTempConnectedNodes().size(); i++) {
            KNode reachableNode = currentNode.getTempConnectedNodes().get(i);
            if (reachableNode.getPathFinderStatus(tracker) == KNode.UNPROCESSED) {
               reachableNode.setParent(currentNode);
               reachableNode.calcHCost(endNode);
               reachableNode.calcGCost();
               reachableNode.calcFCost();
               openList.add(reachableNode);
               reachableNode.setPathFinderStatus(KNode.OPEN, tracker);
            } else if (reachableNode.getPathFinderStatus(tracker) == KNode.OPEN) {
               double currentGCost = reachableNode.getGCost();
               assert reachableNode.getGCost() != KNode.G_COST_NOT_CALCULATED_FLAG;
               double currentNodeToReachableNodeDist = currentNode.getPoint().distance(reachableNode.getPoint());
               double newGCost = currentNode.getGCost() + currentNodeToReachableNodeDist;
               if (newGCost < currentGCost) {
                  reachableNode.setParent(currentNode);
                  reachableNode.setGCost(newGCost);	//reachableNode.calcGCost();
                  reachableNode.calcFCost();
                  // Since the g-cost of the node has changed,
                  // must re-sort the list to reflect this.
                  int index = openList.indexOf(reachableNode);
                  openList.percolateUp(index);
               }
            }
         }
         if (openList.isEmpty()) {
            //System.out.println(this.getClass().getSimpleName()+": openList.size() == 0, returning");
            break;
         }

         currentNode = openList.peekMin();
         if (currentNode == endNode) {
            fillPointList(pointList);
            //System.out.println(this.getClass().getSimpleName()+": currentNode == endNode, returning");
            break;
         }
      }
      clearTempReachableNodes();
      tracker.incrementCountID();
      return pointList;
   }

   protected void clearTempReachableNodes() {
      if (debug) {
         startPointDebug = startNode.getPoint().copy();
         endPointDebug = endNode.getPoint().copy();
         startNodeTempReachableNodesDebug.clear();
         endNodeTempReachableNodesDebug.clear();
         startNodeTempReachableNodesDebug.addAll(startNode.getTempConnectedNodes());
         endNodeTempReachableNodesDebug.addAll(endNode.getTempConnectedNodes());
      }

      // Erase all nodes' tempConnectedNodes
      if (startNode != null) {
         startNode.clearTempConnectedNodes();
      }
      if (endNode != null) {
         endNode.clearTempConnectedNodes();
      }
   }

   protected void fillPointList(List<KPoint> pointList) {
      KNode currentNode = getEndNode();
      while (true) {
         pointList.add(currentNode.getPoint());
         KNode parentNode = currentNode.getParent();
         if (parentNode == null) {
            break;
         }
         currentNode = parentNode;
      }
      Collections.reverse(pointList);
   }

   public boolean pathExists() {
      if (getEndNode() != null && getEndNode().getParent() != null) {
         return true;
      }
      return false;
   }

   public KNode getEndNode() {
      return endNode;
   }

   public KNode getStartNode() {
      return startNode;
   }

   // used only for assertion checks
   protected boolean tempReachableNodesExist(List<PathBlockingObstacle> obstacles) {
      for (int i = 0; i < obstacles.size(); i++) {
         PathBlockingObstacle obst = (PathBlockingObstacle) obstacles.get(i);
         for (int j = 0; j < obst.getNodes().size(); j++) {
            KNodeOfObstacle node = obst.getNodes().get(j);
            if (node.getTempConnectedNodes().size() > 0) {
               return true;
            }
         }
      }
      return false;
   }
}
